package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.Recoverable;

/**
 * @author Jarrod Risley
 */
public class ShooterSubsystem extends Subsystem implements Recoverable {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    final int m_kMaxRetries = 5;
    private int m_shooterMotorCANID;
    protected CANJaguar shooterMotor = null;
    Victor VexVictor = null;
    
    private static final double kIntakeVoltage = -10;
    private static final double kShootVoltage = 8.25;
    private static final double kStopVoltage = 0;
    private static final double kLoaderIn = -1;
    private static final double kLoaderOut = 1;
    private static final double kLoaderStop = 0;
    
    
    
    public static class ShooterValue {

        public int value;
        // Lifter PresetPositions
        protected static final int m_kIngest = 0;
        protected static final int m_kStop = 1;
        protected static final int m_kShoot = 2;
        public static final ShooterValue kIngest = new ShooterValue(m_kIngest);
        public static final ShooterValue kStop = new ShooterValue(m_kStop);
        public static final ShooterValue kShoot = new ShooterValue(m_kShoot);

        private ShooterValue(int output) {
            this.value = output;
        }
    }

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        setDefaultCommand(null);
    }

    public ShooterSubsystem(final int victorPWMPort, final int jaguarCANID) {
        m_shooterMotorCANID = jaguarCANID;

        VexVictor = new Victor(victorPWMPort);

        int tries = 0;
        do {
            try {
                shooterMotor = new CANJaguar(m_shooterMotorCANID);
            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("ShooterSubsystem(): CAN Timeout while creating motor interface for CAN ID " + m_shooterMotorCANID);
                // set this motor to null since it wouldn't create (that way other parts of the class can tell not to use it)
                shooterMotor = null;
            } catch (Exception all) {
                System.out.println(all.toString());
            }
            tries++;
        } while ((null == shooterMotor) && tries < m_kMaxRetries);
        if ((null == shooterMotor)) {
            System.out.println("ShooterSubsystem(): created motor " + m_shooterMotorCANID);
        }

        initialize();

    }

    public void initialize() {

        boolean configured = false;
        int tries = 0;
        do {
            try {
                if (null != shooterMotor) {
                    shooterMotor.changeControlMode(CANJaguar.ControlMode.kVoltage);
                    shooterMotor.configNeutralMode(CANJaguar.NeutralMode.kCoast);
                    shooterMotor.enableControl();
                    shooterMotor.setVoltageRampRate(0.012);
                }
                configured = true;
            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("ShooterSubsystem(): CAN Timeout while confguring shooter motor " + m_shooterMotorCANID);
                // set this motor to null since it wouldn't create (that way other parts of the class can tell not to use it)
            } catch (Exception all) {
                System.out.println(all.toString());
            }
            tries++;
            //System.out.println("Tries equals:" + tries);
        } while (!configured && (tries < m_kMaxRetries));
        if (configured) {
            System.out.println("ShooterSubsystem(): Configured motor " + m_shooterMotorCANID);
        }
    }

    // Ingesting Ball
    public void ingest() {
        VexVictor.set(kLoaderIn);
        if (shooterMotor != null) {  // only command valid motors
            try {
                shooterMotor.setX(kIntakeVoltage); // In volts.
            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("ShooterSubsystem.ingest(): CAN Timeout starting shooter motor " + m_shooterMotorCANID);
            }
        }
    }

    // Stop 
    public void stop() {
        VexVictor.stopMotor();

        if (shooterMotor != null) {  // only command valid motors
            try {
                shooterMotor.setX(kStopVoltage);
            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("ShooterSubsystem.stop(): CAN Timeout while stopping shooter motor " + m_shooterMotorCANID);
            }
        }
        VexVictor.set(kLoaderStop);

    }

    // Shoot -- VexVictor needs to be second to rev up the BaneBot
    public void spinUpShooter() {

        if (shooterMotor != null) {  // only command valid motors
            try {
                shooterMotor.setX(kShootVoltage);
            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("ShooterSubsystem.shoot(): CAN Timeout while starting shooter motor " + m_shooterMotorCANID);
            }
        }
    }

    public void shoot() {
        VexVictor.set(kLoaderOut);
    }

    /*
     * Power drop recovery support
     */
    public void recover(boolean override) {
        boolean needsRecovery = false;

        // figure out if the Jaguar has rebooted
        try {
            if (null != shooterMotor) {
                needsRecovery = shooterMotor.getPowerCycled();
            }
        } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
            System.out.println("ShooterSubsystem(): CAN Timeout while checking for power loss " + m_shooterMotorCANID);
        }

        // and if it did, reinitialize
        if (needsRecovery || override) {
            initialize();
        }
    }
    
    
//    // Calibration Testing Only
//    
//    public void VexOnlyIngest(){
//        VexVictor.set(1);
//    }
//    
//    public void VexOnlyStop() {
//        VexVictor.set(0);
//    }
//    
//    public void JagOnlyIngest() {
//        if (shooterMotor != null) {  // only command valid motors
//            try {
//                shooterMotor.setX(12); // In volts.
//            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
//                System.out.println("ShooterSubsystem.ingest(): CAN Timeout starting shooter motor " + m_shooterMotorCANID);
//            }
//        }
//    }
//    
//    public void JagOnlyStop() {
//        if (shooterMotor != null) {  // only command valid motors
//            try {
//                shooterMotor.setX(kStopVoltage);
//            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
//                System.out.println("ShooterSubsystem.stop(): CAN Timeout while stopping shooter motor " + m_shooterMotorCANID);
//            }
//        }
//    }
}
